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Abstract —In this paper we present a new Kalman filter 
extension for state update called Partitioned Update Kalman 
Filter (PUKF). PUKF updates the state using multidimensional 
measurements in parts. PUKF evaluates the nonlinearity of the 
measurement function within a Gaussian prior by comparing 
the effect of the 2nd order term on the Gaussian measurement 
noise. A linear transformation is applied to measurements to 
minimize the nonlinearity of a part of the measurement. The 
measurement update is then applied using only the part of 
the measurement that has low nonlinearity and the process is 
then repeated for the updated state using the remaining part 
of the transformed measurement until the whole measurement 
has been used. PUKF does the linearizations numerically and no 
analytical differentiation is required. Results show that when the 
measurement geometry allows effective partitioning, the proposed 
algorithm improves estimation accuracy and produces accurate 
covariance estimates. 

I. Introduction 

Bayesian filtering algorithms are used to compute the esti¬ 
mate of an ri-dimensional state x. In a general discrete-time 
model the state evolves according to a state transition equation 

x t = ft(x t - l , St ), (1) 

where f t is the state transition function at time index t, and 
e “ is the state transition noise. The state estimate is updated 
using measurements that are modeled as 

yt = h t (x t ,e v t ), (2) 

where h t is a measurement function and e\ is the measurement 
noise. If the measurement and state transition are linear, noises 
are additive, white and normal distributed, and the prior state 
(xo) is normal distributed, the Kalman update can be used to 
compute the posterior. If these requirements are not fulfilled, 
usually an approximate estimation method has to be used. In 
this work, we concentrate on situations where the noises are 
additive and Gaussian so that (|T]Q take the form 

= ft (x t -i) + £* (3) 

yt = ht(x t ) + e$, (4) 

where e* ~ N(0, Wt), Wit is the state transition noise 
covariance, e\ ~ N(0, Rt), and R t is the measurement noise 
covariance. 

There are two main approaches for computing an approxi¬ 
mation of the posterior distribution: 
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1) Approximate probabilities using point masses (e.g. grid 
and particle filters) 

2) Approximate probabilities by Gaussians (e.g. Kalman 
filter extensions) 

In the first approach one problem is how to choose a good 
number of point masses. The first approach also often requires 
more computational resources than the second approach. A 
drawback of the second approach is that the state distribution 
is assumed normal and unimodal, which makes the estimate 
inaccurate when the true posterior is not normal. Gaussian 
Mixture Filters (GMFs) (a.k.a. Gaussian sum filters) can be 
considered as a hybrid approach that use multiple normal 
distributions to estimate the probability distributions and can 
approximate any probability density function (pdf) Q. GMFs 
have the same kind of problems as the algorithms using 
point masses in choosing a good number of components. The 
algorithm that will be proposed in this paper uses the second 
approach and so we will concentrate on it. 

The algorithms that are based on Gaussian approximations 
usually extend the Kalman filter update to nonlinear mea¬ 
surements (there are also other options, see for example 0, 
||3l). The Extended Kalman Filter (EKF) is a commonly used 
algorithm for estimation with nonlinear measurement models 
0. EKF is based on the first order Taylor linearization of 
the measurement function at the mean of prior. In the Second 
Order Extended Kalman Filter (EKF2) the linearization takes 
also the second order expansion terms into account 0 . In 
contrast to EKF, in EKF2 the prior covariance also affects 
the linearization. Both EKF and EKF2 require analytical 
computation of the Jacobian matrix and EKF2 requires also the 
computation of Hessian matrices of the measurement function. 
In 0 a 2nd order Central Difference Filter (CDF), which can 
be interpreted as a derivative-free numerical approximation 
of EKF2, was presented. The most commonly used Kalman 
filter extension that does not require analytical differentiation 
is probably the Unscented Kalman Filter (UKF) 0 . The 
Gaussian approximations in UKF are based on the propagation 
of “sigma points” through the nonlinear functions. Cubature 
Kalman Filters (CKFs) are similar algorithms, but they have 
different theory in the background 0. All these methods do 
the update as a single operation. 

Some algorithms do multiple linearizations to improve the 
estimate. In [[8] the posterior is computed using multiple EKF 
updates that use different linearization points. In the Iterated 
Extended Kalman Filter (IEKF), the EKF update is computed 
in the prior mean and then the new mean is used as the new 
linearization point 0. This can be done several times. A 
similar update can be done also with other Kalman type filters 
GDI. The Recursive Update Filter (RUF) updates the prior with 
measurement with reduced weight several times HD. In every 
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Fig. 1. Process diagram of the PUKF 


update the linearization point is used from the posterior of the 
last reduced weight update. GMFs can also be considered to 
be filters that do the linearization multiple times, once for each 
Gaussian component, and any Kalman filter extension can be 
used for the update. 

In this paper we present Partitioned Update Kalman Filter 
(PUKF) that updates the state also in several steps. PUKF 
first computes the nonlinearity of measurement models. The 
nonlinearity measure is based on comparing the covariance of 
the 2nd order term covariance of the Gaussian measurement 
noise. Computation of this nonlinearity measure requires the 
same matrices as the EKF2 update and for this we use the 
2nd order CDF 0 , which is a derivative free version of the 
EKF2. 

PUKF applies a linear transformation to the measurement 
function to make a new measurement function that has linearly 
independent measurement noise for measurement elements; 
the smallest nonlinearity corresponding to a measurement ele¬ 
ment is minimized first, then the second smallest nonlinearity 
etc. After the transformation, the update is done using only 
measurement elements that have smaller nonlinearity than a 
set threshold value or using the measurement element with the 
smallest nonlinearity. After the partial measurement update the 
covariance has become smaller or remained the same and the 
linearization errors for remaining measurements may have also 
became smaller. The remaining measurements’ nonlinearity is 
re-evaluated using the partially updated state, the remaining 
measurements are transformed and a new partial update is 
applied until the whole measurement is applied. This process 
is shown in Figure |T] The use of only some dimensions of 
the measurements to get a new prior and the optimization 
of measurement nonlinearities differentiates PUKF from other 
Kalman filter extensions. 

The article is structured as follows: In Section QI] a nu¬ 
merical method for approximate EKF2 update is presented. 
The main algorithm is presented in Section HII] The accuracy 
and reliability of the proposed algorithm is compared with 
other Kalman filter extensions and Particle Filters (PFs) in 
Section UY1 Section [V] concludes the article. 


II. EKF2 and Its Numerical Update Using 2nd 
Order CDF 

Kalman filter extensions, like all Bayesian filters, can be 
computed in two stages: prediction and update. For the state 
transition model © the state is propagated in EKF2 using 
equations 11: 



(5) 

Pf = J f PtiJ fT + + w tt 

(6) 

where fj,^ is the predicted mean at time t, is 

the posterior 

mean of the previous time step, j‘ is the Jacobian of the 

state transition function evaluated at /x^ l5 P t is 

the predicted 

covariance, P^L 1 is the posterior covariance of the previous 

time step and and are defined as 


£/[*]= tr PtX 

(7) 

E{ [lj] =tvP t t 1 H{P+ 1 Hf, 

(8) 

where H / is the Hessian of the ith element 

of the state 

transition function evaluated at nf_ 1 - To simplify the notation 

we do not further show the time indices. 


The update equations of EKF2 for the measurement model 

© are 0 


y- = h(y~) + ^ h 

(9) 

S = J h P~J hT + ^E h + R 

(10) 

K = P~J hT S - 1 

(11) 

n + = n~ + K(y - y~) 

(12) 

P+ =p- - ksk t , 

(13) 

where J h is the Jacobian of the measurement function, K is 
the Kalman gain, S is the innovation covariance, and £ h and 

S" are defined as 


$ = tr P-H? 

(14) 

S|W] = tr P-h£P-H!>, 

(15) 


where H-' is the Hessian matrix of the ith component of the 
measurement function. Eqns (HU can be turned into the EKF 
update using = 0 and S h = 0. 

If the measurement model is linear, the trace terms in 
EKF2 are zero and the update is the optimal update of the 
Kalman filter. When the measurement function is a second 
order polynomial the EKF2 update is not optimal as the 
distributions are no longer Gaussian, but the mean ® and 
innovation covariance ( TTTTb are correct. 

In this paper we use a numerical algorithm to compute an 
EKF2 like update. To derive this algorithm, we start with the 
formulas of the 2nd-order CDF from 0. Let VP~ be a matrix 
such that 

=P~. (16) 

In our implementation this matrix square root is computed 
using Cholesky decomposition. 

Next we define matrices M and Q that are used for 
computing the numerical EKF2 update. We use notation 
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A,; = 7 \/P - [ ; ,j], where VP~[ : .i] is the ith column of matrix 
y/P~ and 7 is an algorithm parameter that defines the spread 
of the function evaluations. Matrix M, whose elements are 


Mr. = 




-ih(n +A i)-h(n -A i) 


(17) 


is needed for the terms with Jacobian. The matrices Qk ~ 
are needed to compute terms with Hessians. 
Elements of Qk are 

= T + Aj) + h[k](fi Aj) 2 h[k]([i )] 

Q k [i,j] =r y 2 [(t[fc](M + Aj + Aj) — /i[fc](/x + Aj) 

(m + Aj) + h^k] (/r )] , i 7^ j- 

(18) 


The EKF2 update can be approximated with these by doing 
the following substitutions: 


tf=tr P-H*? 

s tr Qi 

in © 

(19) 

J h P~J hT ? 

a MM t 

in m 

( 20 ) 

P~J hT p 

s Vp~m t 

in E§ 

( 21 ) 

tr P-H^P-H 1 ? p 

L J 

tr QiQj 

in (fl5l). 

( 22 ) 


The prediction step can be approximated by computing 
Mf dz} and Qi (ITSl) matrices using the state transition 
function instead of the measurement function and doing the 


following substitutions: 




J h P+_ 1 J hT p 

a M f M fT 

in © 

(23) 

tr P~H{ p 

3 tr Q{ 

in © 

(24) 

tr P~ Hf P~Hj p 

a tr Q{Q f . 

in ©. 

(25) 


In da, an update algorithm similar to numerical EKF2 is 
proposed that uses only the diagonal elements of Q matrices. 
They state that 7 = y/Z for Gaussian distributions is optimal 
because it preserves the fourth moment and so we use this 7 
value in our algorithm. 


III. Partitioned Update Kalman Filter 

When the measurement function is linear and the measure¬ 
ment noise covariance is block diagonal, the Kalman update 
produces identical results whether measurements are applied 
one block at a time or all at once. In our approach we try to find 
as linear as possible part of the measurement and use this part 
to update the state estimate to reduce approximation errors in 
the remaining measurement updates. When the measurement 
noise covariance R is not diagonal a linear transformation 
(decorrelation) is applied to transform the measurement so 
that the transformed measurement has diagonal covariance 
ED. In PUKF, we choose this decorrelation so that the 
nonlinearity of the least nonlinear measurement element is 
minimized. The prior is updated using the least nonlinear part 
of the decorrelated measurements. After the partial update 
the process is repeated for the remaining dimensions of the 
transformed measurement. 
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Fig. 2. Probability density functions of sums of independent x 2 and normal 
random variables with different variances 


For measuring the amount of nonlinearity we compare the 
trace term E h with the covariance of the measurement noise: 

d d 

v=t 

T k T < 26 > 

= tr EEVf[‘. ,! i 

i=l k =1 

This nonlinearity measure is a local approximation of the 
nonlinearity and is developed from the measure presented in 
®, d. In na it was compared with other nonlinearity 
measures and it was shown to be a good indication of how 
accurately state can be updated with a nonlinear measurement 
model using a Kalman filter extension. When the measurement 
model is linear the nonlinearity measure is 77 = 0 . 

The matrix E h depends on the nonlinearity of the mea¬ 
surement function and contributes to the innovation covari¬ 
ance (fTOt similarly, but multiplied with as the Gaussian 
measurement noise R. The measure ( 1261 ) compares the ratio 
of Gaussian covariance R and non-Gaussian covariance 3 . 
Figure [2] shows how the pdf of the sum of independent normal 
and \ 2 distributed random variables is closer to normal when 
R > 3 than when R < 5. The 7 2 distribution is chosen in the 
example, because a normal distributed variable squared is x 2 
distributed and in the second order polynomial approximations 
the squared term is the nonlinear part. 

The nonlinearity measure (l26l > can be approximated nu¬ 
merically using the substitution (l22l >. Numerical computation 
of a similar nonlinearity measure was proposed in lH 6 l . but 
the algorithm presented in Section [TT] does the nonlinearity 
computation with fewer measurement function evaluations. 

Multiplying © by an invertible square matrix D gives a 
transformed measurement model 

Dy = Dh{x) + De v . (27) 
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We use the following notations for the transformed measure¬ 
ment model: y = Dy, h(x) = Dh(x), R = DRD T , and 
i v = De v ~ N(0, R). We will show that D can be chosen so 
that 

R = I and tr P~H^P~H^ = 0 ,i£k, (28) 

where and //(' denote the Hessians of the /th and A:th 
element of h{x). 

In ca, it was shown that when a measurement model 
is transformed so that R = I the nonlinearity measure 
(l26l > is equal to the nonlinearity measure of the transformed 
measurements 


^= tr EE^rMi p ~^ p ~^ 

2=1 fc=l 


= ^ E E = tr £ P~H*P~H* 


2=1 k=l 


2=1 


(29) 


Because the cross terms do not affect to the amount of non¬ 
linearity we can extract the nonlinearity caused by individual 
elements of the transformed measurements 


r h = tr P-H^P-H^ (30) 


and the total nonlinearity is 

d 

i = E^- (3i) 

2=1 

In Appendix |A] it is shown that 

3[ij] = [ D ^ h D T } {id] - tr P-H?P-H*. (32) 

In this case the measurement related error terms of the 
transformed measurement R and E /l are diagonal. This makes 
the measurements independent and allows the update of the 
state one element at a time. 

In PUKF nonlinearities are minimized in such a way that 
r]i (l30l > is as small as possible. Then rj 2 is minimized such that 
771 does not change, and 773 so that 771 and 772 do not change 
etc. The decorrelation transformation D that does the desired 
nonlinearity minimization can be computed by first computing 
a matrix square root (IT6t of the measurement noise covariance 

VRVR T = R (33) 

and then an eigendecomposition of vn E ,) \fTi 

UAU t = VR~ 1 E h VR~ T . (34) 


We assume that the eigenvalues in the diagonal matrix A are 
sorted in ascending order. The transformation matrix is now 

D = U t VrT\ (35) 


A proof that this transformation minimizes the nonlinearity 
measures is given in Appendix [B] After transforming the 
measurement model with this matrix, the measurement noise 
covariance is R = I and E h = A. 

After the measurement model is decorrelated (multiplied 
with D), the parts of measurement model that have low non¬ 
linearity (Auji < ^threshold) are used in the update (Section HHi. 


If there is no such part then the most linear element of 
the measurement model is used to update the state. Then 
the same process is repeated for the remaining transformed 
measurement model until the whole measurement is processed. 

In summary the PUKF update is: 

1) Transform the measurement model using D (l35l > 

2) Update the prior using only the least nonlinear measure¬ 
ment elements of the transformed measurement 

3) If there are measurement elements left, use them as new 
measurement and use the updated state as a new prior 
and return to step 1 

The detailed PUKF algorithm is presented in Algorithm Q] and 
a Matlab implementation is available online ED- 

The amount of nonlinearity (l26l > for independent measure¬ 
ments is equal to the sum of the nonlinearities for each of the 
measurements. The quantity ^threshold is compared separately 
to independent transformed measurements elements and, thus, 
we propose to use same ^threshold regardless of the measurement 
dimension. As a rule of thumb the nonlinearity threshold can 
be set to ^threshold = 1, which is equal to the threshold proposed 
for one dimensional measurements in 0. 

Figure [3] shows how PUKF treats a two-dimensional second 
order polynomial measurement function 


y = 


x 2 — 2x — 4 

—x 2 + | j 


+ 


(36) 


where e ~ N(0,/). The prior has mean 1 and covariance 
1. The nonlinearity of each measurement is 4 and the total 

nonlinearity is 8. Then D = ^ ^ 

measurement model has a linear 


1 and the transformed 

term and a polynomial term 


y 


= V2 


x 2 -x-^- 


+ £, 


(37) 


where £ ~ N(0,/). After transformation the first element of 
the measurement function is linear and 771 = 0 and all the 
nonlinearity is associated with the second element 772 = 8. 
In PUKF the linear measurement function is applied first and 
the partially updated state has mean —2 and covariance 2. 
The polynomial measurement function is applied using this 
partially updated state. The amount of nonlinearity for the 
second order polynomial has decreased from 8 to |. EKF2 
applies both measurements at once and the posterior estimate 
is the same for the original and transformed measurement 
models as shown in Appendix [A] When comparing to the true 
posterior, which is computed using a dense grid, the posterior 
estimate of PUKF is significantly more accurate than the EKF2 
posterior estimate. 


IV. Tests 

We compare the proposed PUKF with other Kalman filter 
extensions and a PF in three different test scenarios. The 
PUKF was tested with 4 different values for //threshold- When 
^threshold = oo the whole measurement is applied at once 
and the algorithm is a numerical EKF2. When 77threshoid < 0 
measurement elements are processed one at a time and when 
^threshold = 0 all linear measurement elements are first pro¬ 
cessed together and then nonlinear measurement elements one 
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input : Prior state: fi - mean P - covariance 
Measurement model: y - value, h{ ■) - 
function, R - covariance 
^threshold - nonlinearity limit, 

7 - measurement function evaluation spread 
(default 7 = -\/3) 

output : Updated state: /i - mean, P - covariance 

1 Compute y/R (ITTI i 

2 d t— measurement dimension 

3 while d > 0 do 

4 Compute yfP (IT6t 

s Compute M and Qi, 1 < i < d (1171118b 

6 Compute £ h and E h (fl9b and (l22b 

7 Compute U and A (l34b 

8 D 4- U T VR~ l 

9 Choose largest k so that 


10 

n 

12 

13 

14 

15 

16 
17 


A[i,£] — ^threshold7 ^ ^ ^ ^-[j,j] -' > ^threshold i j -' > k 

if k == 0 then 

k i — 1 

end 

// Compute partial EKF2 update 

y <- D[l:k,:] \h{y) + 1 ^] 

*5 •<— .] + ^[l:k,l:k] + ^ 

/it-/4 + A'(£) [1:fci:] j/-y ) 

Pf- P - KSK T 

// Update remaining measurement 


18 

19 

20 


21 


2/ -D[fc+l:d,:]P 

M®) «- 2)[ fc+ i: di :]/l(a:) 

v7? 4— / // Updated measurement noise 
covariance is an identity matrix 
due to decorrelation 
d <r- d — k // Updated measurement 
dimension 


22 end 

Algorithm 1: Algorithm for doing the measurement update 


in PUKF 


by one. Due to numerical roundoff errors it is better to use 
a small positive ^threshold to achieve this kind of behaviour. In 
our tests we use values {—oo, 0 . 1 , 1 , oo} for threshold- 

EKF and EKF2 are implemented as explained in Section [TT] 
with analytical Jacobians and Hessians. RUF is implemented 
according to m with 3 and 10 steps. IEKF uses 10 iter¬ 
ations. For UKF the values for sigma point parameters are 
a = 10 -3 ,k = 0,/3 = 2. All Kalman filter extensions are 
programmed in Matlab with similar levels of code optimiza¬ 
tions, but the runtimes should still be considered to be only 
indicative. 

For reference we computed estimates with a bootstrap 
particle filter that does systematic resampling at every time 
step ED using various numbers of particles and with a PF 
that uses EKF for computing the proposal distribution ED 
with 10 particles. 

In every test scenario the state transition model is linear 
time-invariant Xt = J^Xt- i +e x , where e x ~ N(0, W). Thus, 



Fig. 3. Transforming second order polynomial measurements to minimize 
nonlinearity of y\ and posterior comparison of PUKF and EKF2 


the prediction step ©I-© can be computed analytically and all 
Kalman filter extensions in tests use the analytical prediction. 

The first test scenario is an aritificial example chosen to 
show the maximal potential of PUKF. The measurement model 
used is 

2s[i] + a; [ 2 ] + X[ 3 ] + + ±xL + 

x m + 2x m + *[3] + Hi] + 3*?2] + h x m 
S[l] + ® [2] + 2®[3] + 14] + \x\ 2] + 14] + ^ y 
*[i] + *[ 2 ] + *p] + x \i] + §®? 2] + 

*[ 1 ] + + h x m + x [2] + h x \3] 

^[1] + x [2\ + x [3\ + \ x m + \ x [1 ] + *[3] 

(38) 

where e v ~ N(0,8/ + 1) and 1 is a matrix of ones. This 
model is a linear transformation of 


h[x) 


*[i] 

*[ 2 ] 

,*[3] 

I r 2 

2‘ T [1] 

I t 2 

fj' 

2 J '[3]. 


+ £ U 


(39) 


where i y ~ N(0,J). The first three elements of ( 139b are 
linear and PUKF with ^threshold €E {0.1,1} uses the three 
linear measurement functions first to update the state. In this 
test scenario the prior mean is at the origin, the prior and 
state transition noise covariances are both 16/, and the state 
transition matrix is an identity matrix. 

Results for positioning with measurement model (l38l > are 
presented in Figure |4] The markers in the upper plot show 
the 5%, 25%, 50%, 75% and 95% quantiles of mean errors 
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Fig. 4. Accuracy of different Kalman filter extensions in estimation with 
second order polynomial measurement model ED- In the top figure markers 
show the 5%, 25%, 50%, 75% and 95% quantiles of errors for each method 
for every estimated step. The errors are computed as the norm of the difference 
of the true and estimated mean. In the bottom figure the markers show how 
often the true state was within estimated error ellipsoids containing 5%, 25%, 
50%, 75% and 95% of the probability mass. 

for each method. The quantiles are computed from 1000 runs 
consisting of 10 steps each. To show the quantiles better a 
logarithmic scale for error is used. PUKF (^threshold < oo) 
is the most accurate of the Kalman filter extensions by a 
large margin. When ^threshold = oc the whole measurement 
is processed at once and the result is the same as with EKF2, 
as expected. In this test scenario the PUKF performs clearly 
the best and methods that use EKF linearizations have very 
large errors. PUKF also outperforms PF with similar runtime. 

In the bottom plot the accuracy of covariance estimates of 
different Kalman filter extensions are compared. For this plot 
we compute how often the true state is within the 5%, 25%, 
50%, 75% and 95% ellipsoids of the Gaussian posterior. That 
is, a true location is within the p ellipsoid when 

Xn {(P - 2drue) T -P _1 (ft ~ Ztrue)) < P, (40) 

where cctme is the true state, /i and P are the posterior mean and 
covariance computed by the filter, and Xn i s 4ic cumulative 
density function of the chi-squared distribution with n degrees 
of freedom. The filter’s error estimate is reliable when markers 
are close to the p values (dotted lines in the Figure). From 
the figure it is evident that PUKF and EKF2 have the most 
reliable error estimates and all other methods have too small 
covariance matrices. 

The EKFPF did not perform wery well. This is probably 
caused by the inconsistency of EKF estimates that were used 


UKF EKF2 



Fig. 5. Example situation of bearings positioning 

as the proposal distribution. We tested EKFPF also with 1000 
particles. The estimation accuracy was similar to that obtained 
with a bootrstrap PF with 1000 particles, but the algorithm was 
much slower than other algorithms. 

In our second test scenario the planar location of a target 
is estimated using bearing measurements. When the target is 
close to the sensor the measurement model is nonlinear, but 
when the target is far away the measurement becomes almost 
linear. The measurement model is 

y = atan2(iE[2] - rppzp] - r^) + e v , (41) 

where atan2 is the four quadrant inverse tangent, r is the 
sensor location, and measurement noises are zero mean inde¬ 
pendent, with standard deviation of 2°. We choose the branch 
of atan2 so that evaluated values are as close as possible to the 
realized measurement value. In the test scenario two bearings 
measurements are used, one from a sensor close to the prior 
and the second from a sensor far away. 

A representative initial state update using UKF, EKF2, RUF 
and PUKF is shown in Figure 0 This example is chosen 
so that the differences between estimates of different filters 
is clearly visible. The red line encloses the same probability 
mass of the true posterior as the 1 • a ellipses (black lines) of 
the Gaussian approximations computed with different Kalman 
filter extensions. The measurement from the distant sensor is 
almost linear within the prior and UKF uses it correctly, but 
the linearization of the estimate from the nearby sensor is not 
good and the resulting posterior is very narrow (EKF would 
be similar). In the EKF2 update the second order term of the 
measurement model from the nearby sensor is so large that 
EKF2 almost completely ignores that measurement and the 
prior is updated using only the measurement from the distant 
sensor. The iterative update of RUF results in an estimate with 
small covariance that has similar shape as the true covariance. 
The mean of the true posterior is not inside the one-sigma 
ellipses of the RUF estimate and the mean is too close to the 
nearby sensor. 
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Fig. 6. Accuracy of different filters in bearings only tracking 


The first transformed measurement used by PUKF is almost 
the same as the measurement from the distant sensor and 
the estimate after the first partial update is similar to the 
EKF2 estimate. Because the estimate updated with the first 
measurement is further away from the nearby sensor the 
linearization of the second measurement is better and the 
posterior estimate is closer to the true posterior than with 
EKF2. The covariance estimate produced by PUKF is more 
conservative than the RUF of UKF covariances. 

Figure [6] shows the statistics for this scenario. For this 
Figure the scenario was ran 1000 times using the same sensor 
locations and 10 step estimation with a 4-dimensional state 
model containing 2 position and 2 velocity dimensions. The 
prior has zero mean and covariance 10/. The state transition 
function is 


f(x) 


I 

0 


I 

I 


X + E 


(42) 


where 


N O, 


—L/ 

T.r 
200 1 



(43) 


Figure [6] shows that the PUKF provides the best accuracy. 
Interestingly RUF with 3 iterations has better accuracy than 
with 20 iterations. From the plot that shows the accuracy of 
the error estimates we can see that the PUKF and EKF2 have 
the best error estimates. Other methods have too optimistic 
covariance estimates. In this test scenario the PF did not 
manage to get good estimates with similar runtimes. 

In the third test scenario we consider bearings only tracking 
with sensors close to each other. Otherwise the measurement 
model is the same as in the previous scenario The prior is as 



Fig. 7. Example of first update in bearings only tracking 


in previous test scenario. The state transition function is also 
(l42t but the state transition noise is higher: 
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(44) 


The initial state and representative first updates are shown 
in Figure [7] In this Figure UKF and RUF estimates have very 
small covariances and so the plots are magnified. The UKF 
estimate mean is closer to the true mean than EKF2 and PUKF 
estimates, but the covariance of the estimate is very small. 
RUF has a better estimate than UKF, but the estimate is biased 
towards the sensor locations. Because both sensors are nearby 
and have large second order terms EKF2 and PUKF estimates 
do not differ much. 

Results for estimating 10 step tracks 1000 times are shown 
in Figure [8] In this case the RUF has the best accuracy. In 
PUKF there is only very small differences whether all of 
the measurement are used at once or a nonlinearity threshold 
is used. This means that in this measurement geometry the 
partitioned update does not improve accuracy. EKF2 has better 
covariance estimates than the numerical update PUKF even 
though it has larger errors. The covariance estimates produced 
by RUF were again too small. In this test the PF has better 
accuracy than the Kalman filter extensions. 

To further evaluate the accuracy of the estimates, we com¬ 
pare Kullback-Leibler (KL) divergences of estimates. The KL 
divergence is defined as 

'p{ x) 


In 


q(x) 


p(x)dx., 


(45) 


where p{x) is the pdf of the true distribution and q(x) is 
the pdf of the approximate distribution lf20l . We computed 
the KL divergence for position dimensions. The true pdf is 
approximated using a 50 x 50 grid. The probability for each 
grid is computed as the sum of particle weights of a PF 
particles within each cell. For this we used 10 6 particles. 

Table H shows the median Kullback-Leibler divergences for 
each method in the two bearings measurement test scenarios. 
































TABLE I 

Median Kullback-Leibler divergences of position dimensions in the two bearings only tests 


Method 

PUKF 

^/threshold = 00 

PUKF 

^/threshold — 0.1 

PUKF 

^/threshold = 1 

PUKF 

^/threshold = 00 

RUF 
r = 20 

RUF 

T = 3 

UKF 

EKF 

IEKF 

T = 10 

EKF2 

First bearings test 

0.62 

0.62 

0.63 

1.07 

0.99 

1.70 

5.20 

14.42 

6.69 

1.16 

Second bearings test 

2.14 

2.14 

2.14 

2.33 

3.56 

2.53 

9.36 

10.97 

8.84 

2.60 



Relative runtime compared to the EKF 



Fig. 8. Results for bearings only tracking with sensors close to each other 

PUKF has the smallest KL divergence in both test scenarios. 

V. Conclusions and Future Work 

In this paper we presented a new extension of the Kalman 
filter: the Partitioned Update Kalman Filter (PUKF). The 
proposed filter evaluates the nonlinearity of a multidimen¬ 
sional measurement and transforms the measurement model 
so that some dimensions of the measurement model have 
as low nonlinearity as possible. PUKF does the update of 
the state using the measurement in parts, so that the parts 
with the smallest amounts of nonlinearity are processed first. 
The proposed algorithm improves estimation results when 
measurements are such that the partial update reduces the 
nonlinearity of the remaining part. According to the simulated 
tests the PUKF improves the estimates when measurements 
can be transformed so that an informative linear part of the 
measurement can be extracted. 

In many practical situations the almost linear part could be 
extracted manually. For example. Global Positioning System 
(GPS) measurements are almost linear and they could be ap¬ 
plied before other measurements. The proposed algorithm does 
the separation automatically and when using the numerical 
algorithm for computing the prediction and update analytical 
differentiation is not required. 


In our tests the estimated covariances produced by EKF2 
and PUKF were the most accurate. In ED it was claimed that 
RUF produces more accurate error estimates than EKF2. Their 
results were based on comparing 3 • a errors in ID estimation. 
In this comparison 92% of samples should be within the 3 • a 
range. For their results they had only 100 samples and from the 
resulting figure it is hard to see how many samples exactly are 
within the range, but for EKF2 most of the points are within 
the range and some are outside. 

In our tests, among other Kalman filter extensions RUF 
had good accuracy, but it provided too small covariance 
matrices. In future it could be interesting to extend RUF CD 
to use EKF2-like statistical second order linearization and then 
combine it with the proposed algorithm. 

Another use case for PUKF would be merging it with the 
Binomial Gaussian mixture filter ED- This filter decorrelates 
measurements and uses nonlinearity measure (|30| > as an in¬ 
dication of whether the measurement model is so nonlinear 
that the prior component should be split. By decorrelating 
measurements with the algorithm proposed in this paper and 
doing the partial updates for the most linear components first, 
unnecessary splits could be avoided. 
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Appendix A 

Invariance of EKF and EKF2 to a linear 

TRANSFORMATION OF THE MEASUREMENT MODEL 

The second order Taylor polynomial approximation of the 
measurement function is 


The terms p' and E h are 


c = 


’tr P~Hf ' 


"t r P ELi D [lM Hk 

tr P~ H h 2 

= 

trP-ELt^P,*]^ 

tr P~H h n 


k P ~T,l=l D [n,k] H k- 


= D 


update-kahnan-pjtjgrj ^ — jjh 


tr P~Hi 
tr P~H!} 


(50) 


= DC 


h{x) =h(n ) + J h (x — fi ) 

(x - n~) T H%(x - fi~) 


.(x-H ) t H^(x-h ) 


+ £ y 


h(x) =Dh(x) = Dh(n ) + DJ h (x — fi ) 

’0 - 0 ~ M - )" 

1 (x - - n~) 

+ 2° . 

{x - fi~) T H%(x - n~)_ 

The transformed Jacobian is 

jh = DJ h 

and ith transformed Hessian is 


De v 


H? = J2 D l*,k] H £- 


Sf. •] =tr P-H*P-H* 


= trP-[Y J D [ilk] H£ P- £ D {iA H* 


\k—l 


v/=l 


(51) 


= E E D m D m tr p-h^p-hI 1 

k= 1 1=1 

=> E h = DE h D T 


For EKF update these terms are replaced with zero matrices. 

Now using these transformed quantities in the EKF2 update 
equations (IWTHi gives 


y- = k^-) + \i h =D(hfa-) + \t h 

S = DJ h P~J hT D T 1 1 nnhnT 


DE n D 1 + DRD 


(52) 

(53) 


= DSD 1 


(46) 


K = P~J h S _1 

= p-J hT D T D- T S~ 1 D- 1 = P-J^S-'D- 1 ( 54 ) 

= KD- 1 


(C — /r + I< 


Dy-Dh(j*-)-±DZ h 

1 


where Jacobian J h and Hessians II h are evaluated at prior 
mean, e v is the measurement function noise. 

In the linear transformation the measurement function 
is multiplied by D. The second order approximation is 


= IT + K{y - JV - 2* ) 


(55) 


(47) 


(48) 


(49) 


k=1 


= 

P+ = p- - ksk T 

= p- KD~ 1 DSD T (KD~ 1 ) T = p~ - i<sk t 

= p + , 

(56) 

which shows that the posterior is the same as with the non- 
transformed measurements. 

Appendix B 

Proof that the nonlinearities are minimized 

Let E h be a diagonal matrix containing nonlinearity values 
ordered ascending on the diagonal and let measurement noise 
covariance matrix be identity matrix R = I. We will show that 
the smallest diagonal element of E h is as small as possible 
under a linear transformation that preserves R = I and 
further that the second smallest diagonal element is as small 
as possible, when the smallest is as small as possible etc. 

If the measurement model is transformed by multiplying it 
with V, the transformed variables are E h = VS h V T and R = 
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VIV T = VV T . Because we want to have R = I, V has to be 
unitary. The /th diagonal element of the transformed matrix 
is vfE h Vi = ^2j=i v 'i [j] “U 7 i ’ where u, is the /th column of 
V. Because V is unitary, we have Y^=i v i[j] = 1 an< ^ the /th 
diagonal element of the transformed matrix ~ h ' 




[j] 5 Lm] 


>£ 

3=1 3=1 

Thus, the new diagonal element cannot be smaller than the 


v i,U] < 57 ) 


smallest diagonal element of H . 

If the smallest element is in the first element of the diagonal 
the possible transformation for the second smallest element is 


(58) 


With the same reasoning as given already the second diagonal 
has to be already the smallest possible. Inductively this applies 
to all diagonal elements. 


1 and the zth 
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'1 o T ' 
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